// Have include guard.
#ifndef PIDENCODERSPEEDCONTROLLER_HPP
#define PIDENCODERSPEEDCONTROLLER_HPP

#include "WPILib.h"

/**
 *  The PidMotorController class is a base class that represents a motor that uses PID control to enable control modes other than percent vbus.
 */
class PidMotorController :
	public SpeedController, public PIDSource
{
	public:
		/**
		 *  The ControlMode enum is used to select how the motor is controlled by the Set function. 
		 */
		enum ControlMode
		{
			/// The PercentVBus control mode sets the percentage of voltage sent to the motor. 
			PercentVBus = CANJaguar::kPercentVbus,
			
			/// The Speed control mode sets the speed that the motor spins. 
			Speed = CANJaguar::kSpeed,
			
			/// The Position control mode sets the position or distance for the motor to spin to. 
			Position = CANJaguar::kPosition,
			
			/// The Voltage control mode sets the voltage sent to the motor.
			Voltage = CANJaguar::kVoltage,
			
			/// The Current control mode sets the current sent to the motor.
			Current = CANJaguar::kCurrent,
		};
		
		/**
		 *  The Pid struct packages P, I, and D values into one object.
		 */
		struct Pid
		{
			public:
				/**
				 *  Constructor for Pid.
				 *  
				 *  @param p The P value.
				 *  @param i The I value.
				 *  @param d The D value.
				 */
				inline Pid(double p, double i, double d)
				{
					m_p = p;
					m_i = i;
					m_d = d;
				}
				
				/**
				 *  Get the P value.
				 *  
				 *  @return The P value.
				 */
				inline double GetP() const
				{
					return m_p;
				}
				
				/**
				 *  Get the I value.
				 *  
				 *  @return The I value.
				 */
				inline double GetI() const
				{
					return m_i;
				}
				
				/**
				 *  Get the D value.
				 *  
				 *  @return The D value.
				 */
				inline double GetD() const
				{
					return m_d;
				}
			private:
				double m_p;
				double m_i;
				double m_d;
		};
		
		virtual void PIDWrite(float output);
		virtual double PIDGet();
		
		/**
		 *  Get the current speed that the motor is running.
		 *  
		 *  Units vary depending on child class because we have not standardized it yet.
		 *  Warning: Child classes may throw an exception if the speed control mode is not supported.
		 *  
		 *  @return The current speed of the motor. 
		 */
		virtual double GetSpeed() = 0;
		
		/**
		 *  Get the current position in units of rotations that the motor has reached since motor was enabled.
		 *  
		 *  Warning: Child classes may throw an exception if the position control mode is not supported.
		 *  
		 *  @return The current position of the motor. 
		 */
		virtual double GetPosition() = 0;
		
		/* The following function declaration is derived from 
		 * WPILib, which is licenced under the licence specified
		 * in "Licence-WPILib.txt".
		 */
		/**
		 *  Enable the motor.
		 *  
		 *  If the parameter initialEncoderPosition is passed, the current position should be treated as initialEncoderPosition. 
		 *  
		 *  @param initialEncoderPosition The initial position of the motor.
		 */
		virtual void Enable(double initialEncoderPosition = 0) = 0;
		
		/**
		 *  Get the current ControlMode that the motor is in.
		 *  
		 *  @return The current ControlMode. 
		 */
		virtual ControlMode GetControlMode() = 0;
		
		/**
		 *  Set the ControlMode.
		 *  
		 *  You may need to call Enable to actually change the control mode.
		 *  Warning: Child classes may throw an exception if mode is not supported.
		 *  
		 *  @param mode The desired ControlMode. 
		 */
		virtual void SetControlMode(ControlMode mode) = 0;
		
		/**
		 *  Set the number of encoder codes per revolution.
		 *  
		 *  Specify the number of encoder "codes" it takes to complete a revolution.
		 *  
		 *  @param codesPerRevolution The number of encoder codes per revolution.
		 */
		virtual void SetEncodersCodesPerRevolution(UINT16 codesPerRevolution) = 0;
		
		/**
		 *  Set the P, I, and D values.
		 *  
		 *  @param p The P value.
		 *  @param i The I value.
		 *  @param d The D value.
		 */
		virtual void SetPid(double p, double i, double d) = 0;
		
		/**
		 *  Set the P, I, and D values.
		 *  
		 *  @param pid A Pid object containing the desired P, I, and D values.
		 */
		inline void SetPid(Pid pid)
		{
			SetPid(pid.GetP(), pid.GetI(), pid.GetD());
		}
		
		/**
		 *  Get the current P value
		 *  
		 *  @return The current P value.
		 */
		virtual double GetP() = 0;
		
		/**
		 *  Get the current I value
		 *  
		 *  @return The current I value.
		 */
		virtual double GetI() = 0;
		
		/**
		 *  Get the current D value
		 *  
		 *  @return The current D value.
		 */
		virtual double GetD() = 0;
};

#endif // #ifndef PIDENCODERSPEEDCONTROLLER_HPP
